﻿// --------------------------------------------------------------------------------------------------------------------
// <copyright file="MultiplePostureDetector.cs" company="arvystate.net">
//   arvystate.net
// </copyright>
// <summary>
//   Similar posture detector to Kinect Toolbox, except supporting
// </summary>
// --------------------------------------------------------------------------------------------------------------------

namespace PresentationAid.Lib.Gestures
{
    using System;
    using System.Collections.Generic;

    using Kinect.Toolbox;

    using Microsoft.Kinect;

    using NLog;

    /// <summary>
    /// Similar posture detector to Kinect Toolbox, except with multiple
    /// skeleton support.
    /// </summary>
    public class MultiplePostureDetector
    {
        #region Static Fields

        /// <summary>
        /// The logger.
        /// </summary>
        private static readonly Logger Logger = LogManager.GetCurrentClassLogger();

        #endregion

        #region Fields

        /// <summary>
        /// The accumulated posture.
        /// </summary>
        private readonly Dictionary<int, string> accumulatedPosture;

        /// <summary>
        /// The accumulator.
        /// </summary>
        private readonly Dictionary<int, int> accumulator;

        /// <summary>
        /// The accumulator target.
        /// </summary>
        private readonly int accumulatorTarget;

        /// <summary>
        /// The previous posture.
        /// </summary>
        private readonly Dictionary<int, string> previousPosture;

        #endregion

        #region Constructors and Destructors

        /// <summary>
        /// Initializes a new instance of the <see cref="MultiplePostureDetector"/> class.
        /// </summary>
        public MultiplePostureDetector()
        {
            this.accumulatorTarget = 10;
            this.Epsilon = 0.1f;
            this.MaxRange = 0.25f;

            this.accumulatedPosture = new Dictionary<int, string>();
            this.accumulator = new Dictionary<int, int>();
            this.previousPosture = new Dictionary<int, string>();
        }

        #endregion

        #region Public Events

        /// <summary>
        /// The posture detected.
        /// </summary>
        public event Action<string, int> PostureDetected;

        #endregion

        #region Public Properties

        /// <summary>
        /// Gets or sets the epsilon.
        /// </summary>
        public float Epsilon { get; set; }

        /// <summary>
        /// Gets or sets the max range.
        /// </summary>
        public float MaxRange { get; set; }

        #endregion

        #region Public Methods and Operators

        /// <summary>
        /// Method tracks skeleton postures
        /// </summary>
        /// <param name="skeleton">
        /// The skeleton.
        /// </param>
        public void TrackPostures(Skeleton skeleton)
        {
            if (skeleton.TrackingState != SkeletonTrackingState.Tracked)
            {
                return;
            }

            if (!this.accumulator.ContainsKey(skeleton.TrackingId))
            {
                this.accumulator.Add(skeleton.TrackingId, 0);
                this.accumulatedPosture.Add(skeleton.TrackingId, string.Empty);
                this.previousPosture.Add(skeleton.TrackingId, string.Empty);
            }

            Vector3? headPosition = null;
            Vector3? leftHandPosition = null;
            Vector3? rightHandPosition = null;

            foreach (Joint joint in skeleton.Joints)
            {
                if (joint.TrackingState != JointTrackingState.Tracked)
                {
                    continue;
                }

                switch (joint.JointType)
                {
                    case JointType.Head:
                        headPosition = joint.Position.ToVector3();
                        break;
                    case JointType.HandLeft:
                        leftHandPosition = joint.Position.ToVector3();
                        break;
                    case JointType.HandRight:
                        rightHandPosition = joint.Position.ToVector3();
                        break;
                }
            }

            // HandsJoined
            if (this.CheckHandsJoined(rightHandPosition, leftHandPosition))
            {
                this.RaisePostureDetected("HandsJoined", skeleton.TrackingId);
                return;
            }

            // LeftHandOverHead
            if (this.CheckHandOverHead(headPosition, leftHandPosition))
            {
                this.RaisePostureDetected("LeftHandOverHead", skeleton.TrackingId);
                return;
            }

            // RightHandOverHead
            if (this.CheckHandOverHead(headPosition, rightHandPosition))
            {
                this.RaisePostureDetected("RightHandOverHead", skeleton.TrackingId);
                return;
            }

            // LeftHello
            if (this.CheckHello(headPosition, leftHandPosition))
            {
                this.RaisePostureDetected("LeftHello", skeleton.TrackingId);
                return;
            }

            // RightHello
            if (this.CheckHello(headPosition, rightHandPosition))
            {
                this.RaisePostureDetected("RightHello", skeleton.TrackingId);
                return;
            }

            this.Reset(skeleton.TrackingId);
        }

        #endregion

        #region Methods

        /// <summary>
        /// The raise posture detected.
        /// </summary>
        /// <param name="posture">
        /// The posture.
        /// </param>
        /// <param name="trackingId">
        /// The tracking id.
        /// </param>
        protected void RaisePostureDetected(string posture, int trackingId)
        {
            Logger.Trace(
                "Method: RaisePostureDetected - Posture: {0}, TrackingId: {1}, AccumulatorCount: {2}, Accumulated: {3}, Previous: {4}", 
                posture, 
                trackingId, 
                this.accumulator[trackingId], 
                this.accumulatedPosture[trackingId], 
                this.previousPosture[trackingId]);

            if (this.accumulator[trackingId] < this.accumulatorTarget)
            {
                if (this.accumulatedPosture[trackingId] != posture)
                {
                    this.accumulator[trackingId] = 0;
                    this.accumulatedPosture[trackingId] = posture;
                }

                this.accumulator[trackingId]++;
                return;
            }

            if (this.previousPosture[trackingId] == posture)
            {
                return;
            }

            this.previousPosture[trackingId] = posture;

            if (this.PostureDetected != null)
            {
                Logger.Trace("MethodDetail: RaisePostureDetected - PostureDetected executed");

                this.PostureDetected(posture, trackingId);
            }

            this.accumulator[trackingId] = 0;
        }

        /// <summary>
        /// The reset.
        /// </summary>
        /// <param name="trackingId">
        /// The tracking id.
        /// </param>
        protected void Reset(int trackingId)
        {
            this.accumulator[trackingId] = 0;
            this.previousPosture[trackingId] = string.Empty;
        }

        /// <summary>
        /// The check hand over head.
        /// </summary>
        /// <param name="headPosition">
        /// The head position.
        /// </param>
        /// <param name="handPosition">
        /// The hand position.
        /// </param>
        /// <returns>
        /// The <see cref="bool"/>.
        /// </returns>
        private bool CheckHandOverHead(Vector3? headPosition, Vector3? handPosition)
        {
            if (!handPosition.HasValue || !headPosition.HasValue)
            {
                return false;
            }

            if (handPosition.Value.Y < headPosition.Value.Y)
            {
                return false;
            }

            if (Math.Abs(handPosition.Value.X - headPosition.Value.X) > this.MaxRange)
            {
                return false;
            }

            if (Math.Abs(handPosition.Value.Z - headPosition.Value.Z) > this.MaxRange)
            {
                return false;
            }

            return true;
        }

        /// <summary>
        /// The check hands joined.
        /// </summary>
        /// <param name="leftHandPosition">
        /// The left hand position.
        /// </param>
        /// <param name="rightHandPosition">
        /// The right hand position.
        /// </param>
        /// <returns>
        /// The <see cref="bool"/>.
        /// </returns>
        private bool CheckHandsJoined(Vector3? leftHandPosition, Vector3? rightHandPosition)
        {
            if (!leftHandPosition.HasValue || !rightHandPosition.HasValue)
            {
                return false;
            }

            float distance = (leftHandPosition.Value - rightHandPosition.Value).Length;

            if (distance > this.Epsilon)
            {
                return false;
            }

            return true;
        }

        /// <summary>
        /// The check hello.
        /// </summary>
        /// <param name="headPosition">
        /// The head position.
        /// </param>
        /// <param name="handPosition">
        /// The hand position.
        /// </param>
        /// <returns>
        /// The <see cref="bool"/>.
        /// </returns>
        private bool CheckHello(Vector3? headPosition, Vector3? handPosition)
        {
            if (!handPosition.HasValue || !headPosition.HasValue)
            {
                return false;
            }

            if (Math.Abs(handPosition.Value.X - headPosition.Value.X) < this.MaxRange)
            {
                return false;
            }

            if (Math.Abs(handPosition.Value.Y - headPosition.Value.Y) > this.MaxRange)
            {
                return false;
            }

            if (Math.Abs(handPosition.Value.Z - headPosition.Value.Z) > this.MaxRange)
            {
                return false;
            }

            return true;
        }

        #endregion
    }
}